//
//  main.cpp
//  opencv_A
//
//  Created by 杨经纬 on 2022/2/8.
//


//路径opencv1





#include <iostream>
#include <math.h>
#include <string>
#include <vector>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>



using namespace std;
using namespace cv;
//
Mat img;
Mat imgGray;
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
//vector<RotatedRect> minRect( contours.size() );
//vector<RotatedRect> minEllipse( contours.size() );
vector<Point2f> Point2D;
vector<Point3f> Point3D;
class CAB{
public:
    Point2f rect_points[4];
};

CAB QSD[50];



void Image_Gray(Mat&);
void Outline(Mat&);
void PNP(Mat&,Mat&);

int main(int argc, const char * argv[]) {
    //初始化相机参数Opencv
        double camD[9] = {
            6800.7, 0, 3065.8,
            0, 6798.1, 1667.6,
            0, 0, 1 };
        cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, camD);

        //畸变参数
        double distCoeffD[5] = { -0.189314, 0.444657, -0.00116176, 0.00164877, -2.57547 };
        cv::Mat distortion_coefficients = cv::Mat(5, 1, CV_64FC1, distCoeffD);

    Point3D.push_back(Point3f(0,0,0));
    Point3D.push_back(Point3f(0,58,0));
    Point3D.push_back(Point3f(7,58,0));
    Point3D.push_back(Point3f(7,0,0));
    //三维坐标



    VideoCapture camera(0);
    if(!camera.isOpened())return 1;
    while(1){
       // img = imread("opencv1/ssss.jpeg");
    imshow("ss", img);
        //camera >> img;
        Image_Gray(img);
        imshow("sd", imgGray);
        Outline(imgGray);
        imshow("s", img);
        PNP(camera_matrix,distortion_coefficients);
        waitKey(0);
    }
    return 0;
}


void Image_Gray(Mat& img){
    Mat imgcolor;
    Mat Binarization;
    vector<Mat> counters;
    split(img, counters);
    imgcolor = counters.at(0)*1.0+counters.at(2)*1.0;              // - counters.at(2)*0.4;
    threshold(imgcolor, Binarization, 160, 255, THRESH_BINARY);
    Mat kernel = getStructuringElement(MORPH_RECT, Size(3,3));
    medianBlur(Binarization, imgGray, 3);
//    dilate(Binarization, Binarization, kernel);
//    erode(Binarization, imgGray, kernel);
}

void Outline(Mat& imgGray){
    findContours(imgGray, contours, hierarchy,RETR_LIST, CHAIN_APPROX_NONE);
    vector<RotatedRect> minRect( contours.size() );
     vector<RotatedRect> minEllipse( contours.size() );


     for( int i = 0; i < contours.size(); i++ )
        { minRect[i] = minAreaRect( Mat(contours[i]) );
//          if( contours[i].size() > 5 )
//            { minEllipse[i] = fitEllipse( Mat(contours[i]) ); }
        }


     /// 绘出轮廓及其可倾斜的边界框和边界椭圆
    int KEY;
     for( int i = 0; i< minRect.size(); i++ )
        {
          //Scalar color = Scalar( imgGray.uniform(0, 255), imgGray.uniform(0,255), imgGray.uniform(0,255) );
          // contour
     //     drawContours( drawing, contours, i, Scalar(255,255,255), 1, 8, vector<Vec4i>(), 0, Point() );
          // ellipse
    //      ellipse( drawing, minEllipse[i], Scalar(255,255,255), 2, 8 );
          // rotated rectangle
            if(minRect[i].angle<20&&minRect[i].size.width/minRect[i].size.height<0.5){
           minRect[i].points( QSD[i].rect_points );
                KEY = i;
          for( int j = 0; j < 4; j++ )
             line( img, QSD[i].rect_points[j], QSD[i].rect_points[(j+1)%4], Scalar(0,0,255), 5, 8 );
            }
            if(minRect[i].angle>70&&minRect[i].size.width/minRect[i].size.height>6.0){
                 minRect[i].points( QSD[i].rect_points );
                KEY = i;
                Point2f A = QSD[i].rect_points[0];
                QSD[i].rect_points[0] = QSD[i].rect_points[3];
                QSD[i].rect_points[3] = QSD[i].rect_points[2];
                QSD[i].rect_points[2] = QSD[i].rect_points[1];
                QSD[i].rect_points[1] = A;
                for( int j = 0; j < 4; j++ )
                   line( img,QSD[i].rect_points[j], QSD[i].rect_points[(j+1)%4], Scalar(0,0,255), 5, 8 );
            }
        }
            for( int j = 0; j < 4; j++ )Point2D.push_back(QSD[KEY].rect_points[j]);



     /// 结果在窗体中显示
//    namedWindow( "Contours", WINDOW_AUTOSIZE );
//    imshow( "Contours", drawing );
}


void PNP(Mat& camera_matrix,Mat& distortion_coefficients ){
    Mat rvec;
    Mat tvec;
    solvePnP(Point3D, Point2D, camera_matrix, distortion_coefficients, rvec, tvec,false,SOLVEPNP_P3P);
    Mat romat;
   // Rodrigues(rvec, romat);
    cout << -tvec.at<double>()<<"mm";
    
}


